
/// \file MappingHelpers.cpp
/// \brief Helper functions for mapping operation such as (load and initialize vector maps , convert map from one format to another, .. )
/// \author Hatem Darweesh
/// \date Jul 2, 2016



#include "mapping/MappingHelpers.h"
#include <float.h>
#include <fstream>
#include <algorithm>

// #define ACCEPT_USE_OF_DEPRECATED_PROJ_API_H
#include <proj.h>

namespace op {
	namespace mapping {

using namespace std;
using namespace planning;
using namespace utilities;

constexpr int ANGLE_MAX_FOR_DIRECTION_CHECK = 15;
constexpr double MAX_DISTANCE_TO_START_LANE_DETECTION = 100;

MappingHelpers::MappingHelpers() {
}

MappingHelpers::~MappingHelpers() {
}


void MappingHelpers::llaToxyz_proj(const std::string& proj_str, const WayPoint& origin, const double& lat,
		const double& lon, const double& alt, double& x_out, double& y_out, double& z_out)
{
	if(proj_str.size() < 8) return;

	PJ_CONTEXT *C = proj_context_create();
	PJ *P = proj_create_crs_to_crs (C, "EPSG:4326", proj_str.c_str(), NULL);

	if(P == 0) return;

	PJ_COORD gps_degrees = proj_coord (lat, lon, alt, 0);
	PJ_COORD xyz_out = proj_trans (P, PJ_FWD, gps_degrees);
	x_out = xyz_out.enu.e + origin.pos.x;
	y_out = xyz_out.enu.n + origin.pos.y;
	z_out = xyz_out.enu.u + origin.pos.z;

	proj_destroy (P);
    proj_context_destroy (C); /* may be omitted in the single threaded case */
}

} /* namespace mapping */
} /* namespace op */



/// \file MappingHelpers.h
/// \brief Helper functions for mapping operation such as (load and initialize vector maps , convert map from one format to another, .. )
/// \author Hatem Darweesh
/// \date Jul 2, 2016



#ifndef MAPPINGHELPERS_H_
#define MAPPINGHELPERS_H_

#include "planning/RoadNetwork.h"
#include "utilities/data_rw.h"
#include <math.h>
#include <tinyxml.h>

namespace op {
	namespace mapping {


class MappingHelpers {
public:
	MappingHelpers();
	virtual ~MappingHelpers();

	static void llaToxyz_proj(const std::string& proj_str, const WayPoint& origin, const double& lat,
			const double& lon, const double& alt, double& x_out, double& y_out, double& z_out);

	static void xyzTolla_proj(const std::string& proj_str, const WayPoint& origin, const double& x_in,
			const double& y_in, const double& z_in, double& lat, double& lon, double& alt);
};

} /* namespace mapping */
} /* namespace op */

#endif /* MAPPINGHELPERS_H_ */
